
/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
 */
package be.ac.ua.pats.thesisrobotica.robot.main;

//~--- non-JDK imports --------------------------------------------------------

import be.ac.ua.pats.thesisrobotica.robot.RR.Reflective;
import be.ac.ua.pats.thesisrobotica.robot.RR.Refractive;
import be.ac.ua.pats.thesisrobotica.robot.RR.sensors.Actuator;
import be.ac.ua.pats.thesisrobotica.robot.RR.sensors.Battery;
import be.ac.ua.pats.thesisrobotica.robot.RR.sensors.Sensor;
import be.ac.ua.pats.thesisrobotica.robot.RR.sensors.TouchSensor;
import be.ac.ua.pats.thesisrobotica.robot.RR.sensors.UltrasonicSensor;
import be.ac.ua.pats.thesisrobotica.robot.RR.sensors.listeners.threads.ListenerThread;
import be.ac.ua.pats.thesisrobotica.robot.benchmark.PowerUsage;
import be.ac.ua.pats.thesisrobotica.robot.communication.BluetoothConnection;
import be.ac.ua.pats.thesisrobotica.robot.communication.Connection;
import be.ac.ua.pats.thesisrobotica.robot.constants.RobotConstants;
import be.ac.ua.pats.thesisrobotica.robot.exceptions.InitializationException;
import be.ac.ua.pats.thesisrobotica.robot.exceptions.InvalidConnectionException;
import be.ac.ua.pats.thesisrobotica.robot.exceptions.RobotInitializationException;
import be.ac.ua.pats.thesisrobotica.robot.exceptions.SensorNotFoundException;

import lejos.nxt.Motor;
import lejos.nxt.MotorPort;
import lejos.nxt.NXT;
import lejos.nxt.SensorPort;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.RConsole;

//~--- JDK imports ------------------------------------------------------------

import java.io.IOException;
import java.io.StringWriter;

import java.util.Enumeration;
import java.util.Hashtable;

/**
 * Abstract robot
 *
 * @author Jimmy
 */
public class Robot extends RobotConstants.Robot {
    public static Sensor battery     = null;
    public static Sensor motorPortA  = null;
    public static Sensor motorPortB  = null;
    public static Sensor motorPortC  = null;
    public static Sensor sensorPort1 = null;
    public static Sensor sensorPort2 = null;
    public static Sensor sensorPort3 = null;
    public static Sensor sensorPort4 = null;
    private Connection   connection;
    private boolean      initialized;
    private Hashtable    sensorList;

    public Robot() {
        ID         = Bluetooth.getLocalAddress();
        sensorList = new Hashtable();
    }

    public void initialize() {
        detectSensors();
        startSensorListening();

        initialized = true;
    }

    void startSensorListening() {
        ListenerThread.get().start();
        battery.setTimeInterval(2000);
        battery.setThreshold(1000);
        battery.startListening();

        if (sensorPort1 != null) {
            RConsole.println("Robot::startSensorListening:Setting time interval for sensorPort1");
            sensorPort1.setTimeInterval(3000);
            RConsole.println("Robot::startSensorListening:Start listening sensorPort1");
            sensorPort1.startListening();
        }

        if (sensorPort2 != null) {
            RConsole.println("Robot::startSensorListening:Setting time interval for sensorPort2");
            sensorPort2.setTimeInterval(3000);
            RConsole.println("Robot::startSensorListening:Setting threshold for sensorPort2");
            sensorPort2.setThreshold(1000);
            RConsole.println("Robot::startSensorListening:Start listening sensorPort2");
            sensorPort2.startListening();
        }

        if (sensorPort3 != null) {
            RConsole.println("Robot::startSensorListening:Setting time interval for sensorPort3");
            sensorPort3.setTimeInterval(3000);
            RConsole.println("Robot::startSensorListening:Setting threshold interval for sensorPort3");
            sensorPort3.setThreshold(10);
            RConsole.println("Robot::startSensorListening:Start listening sensorPort3");
            sensorPort3.startListening();
        }

        if (sensorPort4 != null) {
            sensorPort4.startListening();
        }

        if(motorPortA != null) {
            motorPortA.setTimeInterval(3000);
            motorPortA.startListening();
        }         if(motorPortB != null) {
            motorPortB.setTimeInterval(3000);
            motorPortB.startListening();
        }           if(motorPortC != null) {
            motorPortC.setTimeInterval(3000);
            motorPortC.startListening();
        }
    }

    void printStatus() {
        RConsole.println("Current status of the robot: ");
        RConsole.println("Robot ID: " + ID);
        RConsole.println("Sensors:");
        RConsole.println("Sensorport 1:");

        if (sensorPort1 != null) {
            RConsole.println("Type: " + sensorPort1.getType());
            RConsole.println("ID: " + sensorPort1.getId());
            RConsole.println("Interval: " + sensorPort1.getInterval());
            RConsole.println("Threshold: " + sensorPort1.getThreshold());
        } else {
            RConsole.println("Nothing connected");
        }

        RConsole.println("Sensorport 2:");

        if (sensorPort2 != null) {
            RConsole.println("Type: " + sensorPort2.getType());
            RConsole.println("ID: " + sensorPort2.getId());
            RConsole.println("Interval: " + sensorPort2.getInterval());
            RConsole.println("Threshold: " + sensorPort2.getThreshold());
        } else {
            RConsole.println("Nothing connected");
        }

        RConsole.println("Sensorport 3:");

        if (sensorPort3 != null) {
            RConsole.println("Type: " + sensorPort3.getType());
            RConsole.println("ID: " + sensorPort3.getId());
            RConsole.println("Interval: " + sensorPort3.getInterval());
            RConsole.println("Threshold: " + sensorPort3.getThreshold());
        } else {
            RConsole.println("Nothing connected");
        }

        RConsole.println("Sensorport 4:");

        if (sensorPort4 != null) {
            RConsole.println("Type: " + sensorPort4.getType());
            RConsole.println("ID: " + sensorPort4.getId());
            RConsole.println("Interval: " + sensorPort4.getInterval());
            RConsole.println("Threshold: " + sensorPort4.getThreshold());
        } else {
            RConsole.println("Nothing connected");
        }

        RConsole.println("Motorport A: ");

        if (motorPortA != null) {
            RConsole.println("Type: " + motorPortA.getType());
            RConsole.println("ID: " + motorPortA.getId());
            RConsole.println("Interval: " + motorPortA.getInterval());
            RConsole.println("Threshold: " + motorPortA.getThreshold());
        } else {
            RConsole.println("Nothing connected");
        }

        RConsole.println("Motorport B: ");

        if (motorPortB != null) {
            RConsole.println("Type: " + motorPortB.getType());
            RConsole.println("ID: " + motorPortB.getId());
            RConsole.println("Interval: " + motorPortB.getInterval());
            RConsole.println("Threshold: " + motorPortB.getThreshold());
        } else {
            RConsole.println("Nothing connected");
        }

        RConsole.println("Motorport C: ");

        if (motorPortC != null) {
            RConsole.println("Type: " + motorPortC.getType());
            RConsole.println("ID: " + motorPortC.getId());
            RConsole.println("Interval: " + motorPortC.getInterval());
            RConsole.println("Threshold: " + motorPortC.getThreshold());
        } else {
            RConsole.println("Nothing connected");
        }
    }

    public void setNotifications(boolean b) {
        if (sensorList.keys().hasMoreElements()) {
            Enumeration keys = sensorList.keys();
            String      key  = "";

            while (keys.hasMoreElements()) {
                key = (String) keys.nextElement();

                RConsole.println("Robot:setNotifications: Setting notifications: " + b);
                ((Sensor) sensorList.get(key)).setNotifications(b);
            }
        }
    }

    public void detectSensors() {
        battery = new Battery();

        addSensor(battery);

        sensorPort1 = new TouchSensor(SensorPort.S1);

        addSensor(sensorPort1);

        sensorPort2 = new UltrasonicSensor(SensorPort.S2);

        addSensor(sensorPort2);

        try {
            motorPortA = new Actuator(MotorPort.A);

            addSensor(motorPortA);
        } catch (Exception e) {
            RConsole.println("No motor attached to A");
        }

        try {
            motorPortB = new Actuator(MotorPort.B);

            addSensor(motorPortB);
        } catch (Exception e) {
            RConsole.println("No motor attached to B");
        }

        try {
            motorPortC = new Actuator(MotorPort.C);

            addSensor(motorPortC);
        } catch (Exception e) {
            RConsole.println("No motor attached to B");
        }
    }

    public boolean connect() {
        RConsole.println("Robot: Creating new BT connection");

        connection = new BluetoothConnection(this);

        RConsole.println("Robot: trying to connect");
        (new Thread(connection)).start();

        if (sensorList.keys().hasMoreElements()) {
            Enumeration keys = sensorList.keys();
            String      key  = "";

            while (keys.hasMoreElements()) {
                key = (String) keys.nextElement();

                ((Sensor) sensorList.get(key)).setConnection(connection);
            }
        }

        return true;
    }

    public void sendId() throws InvalidConnectionException {
        if (!connection.isConnected()) {
            throw new InvalidConnectionException();
        }

        connection.send("RID," + lejos.nxt.comm.Bluetooth.getAddress());
    }

    public boolean disconnect() {
        if (connection.isConnected()) {
            connection.disconnect();

            return true;
        }

        return false;
    }

    public boolean addSensor(Sensor sensor) {
        if ((sensorList.get(sensor.getId()) == null)) {
            RConsole.println("Robot::addSensor:Adding sensor " + sensor.getId());
            sensorList.put(sensor.getId(), sensor);
            RConsole.println("Robot::addSensor:Adding sensor finished");

            return true;
        }

        return false;
    }

    public void sendBenchmark(String id)
            throws InterruptedException, SensorNotFoundException, InitializationException,
                   RobotInitializationException {
        if (initialized) {
            Sensor sensor = (Sensor) sensorList.get(id);

            if (sensor != null) {
                PowerUsage benchMark = new PowerUsage(sensor);

                benchMark.startTest(100, 100);
                benchMark.send(connection);
            } else {
                throw new SensorNotFoundException(id);
            }
        } else {
            throw new RobotInitializationException();
        }
    }

    public void sendValue(String id)
            throws SensorNotFoundException, InitializationException, RobotInitializationException {
        if (initialized) {
            Sensor sensor = (Sensor) sensorList.get(id);

            if (sensor != null) {
                connection.send(sensor.getId() + "," + ((Reflective) sensor).getValue());
            } else {
                throw new SensorNotFoundException(id);
            }
        } else {
            throw new RobotInitializationException();
        }
    }

    public void setValue(String id, String value)
            throws SensorNotFoundException, InitializationException, RobotInitializationException {
        if (initialized) {
            Sensor sensor = (Sensor) sensorList.get(id);

            if (sensor != null) {
                ((Refractive) sensor).setValue(value);
            } else {
                throw new SensorNotFoundException(id);
            }
        } else {
            throw new RobotInitializationException();
        }
    }

    public void setInterval(String id, String interval)
            throws SensorNotFoundException, InitializationException, RobotInitializationException {
        if (initialized) {
            Sensor sensor = (Sensor) sensorList.get(id);

            if (sensor != null) {
                sensor.setTimeInterval(Integer.parseInt(interval));
            } else {
                throw new SensorNotFoundException(id);
            }
        } else {
            throw new RobotInitializationException();
        }
    }

    public void setThreshold(String id, String threshold) throws SensorNotFoundException, RobotInitializationException {
        if (initialized) {
            Sensor sensor = (Sensor) sensorList.get(id);

            if (sensor != null) {
                sensor.setThreshold(Integer.parseInt(threshold));
            }
        } else {
            throw new RobotInitializationException();
        }
    }

    public void sendThreshold(String id) throws SensorNotFoundException, RobotInitializationException {
        if (initialized) {
            Sensor sensor = (Sensor) sensorList.get(id);

            if (sensor != null) {
                connection.send(String.valueOf(sensor.getThreshold()));
            } else {
                throw new SensorNotFoundException(id);
            }
        } else {
            throw new RobotInitializationException();
        }
    }

    public void sendInterval(String id) throws SensorNotFoundException, RobotInitializationException {
        if (initialized) {
            Sensor sensor = (Sensor) sensorList.get(id);

            if (sensor != null) {
                connection.send(String.valueOf(sensor.getInterval()));
            } else {
                throw new SensorNotFoundException(id);
            }
        } else {
            throw new RobotInitializationException();
        }
    }

    public void sendSensors() throws IOException, RobotInitializationException {
        if (initialized) {
            StringWriter writer = new StringWriter();
            Sensor       sensor;

            writer.append("SID");

            Enumeration keys = sensorList.keys();

            while (keys.hasMoreElements()) {
                writer.append(",");

                String currentId = (String) keys.nextElement();

                writer.append(currentId);
                writer.append(",");

                sensor = (Sensor) sensorList.get(currentId);

                writer.append(sensor.getType());
            }

            RConsole.println("Going to send sensors: " + writer.toString());
            connection.send(writer.toString());
        } else {
            throw new RobotInitializationException();
        }
    }

    public void shutDown(int miliseconds) {
        try {
            Thread.sleep(miliseconds);
            NXT.shutDown();
        } catch (InterruptedException ex) {
            RConsole.println("InterruptedException in Robot:shutdown");
        }
    }
}
